#include <slam_2d_ros/common/frame.h>

namespace slam_2d{

    // 打印数据
    string PrintData(Pose2D pose){
        double x = pose.translation().x();
        double y = pose.translation().y();
        Eigen::Rotation2Dd rotation(pose.rotation());
        double theta = rotation.angle(); // 弧度

        // 使用字符串流拼接信息
        std::ostringstream oss;
        oss.precision(6); // 设置精度
        oss << std::fixed; // 固定浮点格式
        oss << "x: " << x << ", y: " << y << ", angle (rad): " << theta;
        oss << " | (deg): " << theta * 180.0 / M_PI;

        // 返回字符串
        return oss.str();    
    }

};  // namespace slam_2d